#include "arduino.h"

#include "movement.h"
#include "config.h"

using namespace hexapod::config;
using namespace hexapod;

const double SIN30 =  0.5;
const double  COS30 =  0.866;
const double  SIN45  = 0.7071;
const double  COS45 =  0.7071;
const double  SIN15 =  0.2588;
const double  COS15 =  0.9659;

const double   STANDBY_Z  = (kLegJoint3ToTip*COS15-kLegJoint2ToJoint3*SIN30);
const double   LEFTRIGHT_X= (kLegMountLeftRightX+kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15);
const double OTHER_X =(kLegMountOtherX + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*COS45);
const double OTHER_Y =(kLegMountOtherY + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*SIN45);

const double  P1X   =  OTHER_X;
const double  P1Y   =  OTHER_Y;
const double  P1Z  =   -STANDBY_Z;

const double  P2X   =  LEFTRIGHT_X;
const double  P2Y  =   0;
const double  P2Z  =   -STANDBY_Z;

const double  P3X  =   OTHER_X;
const double  P3Y  =   -OTHER_Y;
const double  P3Z  =   -STANDBY_Z;

const double  P4X  =   -OTHER_X;
const double  P4Y  =   -OTHER_Y;
const double  P4Z  =   -STANDBY_Z;

const double  P5X  =   -LEFTRIGHT_X;
const double  P5Y   =  0;
const double  P5Z  =   -STANDBY_Z;

const double  P6X  =   -OTHER_X;
const double  P6Y  =   OTHER_Y;
const double  P6Z  =   -STANDBY_Z;

namespace hexapod {

    namespace {
        const Locations kStandby {
            {P1X+0.0, P1Y, P1Z}, {P2X, P2Y, P2Z}, {P3X, P3Y, P3Z}, {P4X, P4Y, P4Z}, {P5X, P5Y, P5Z}, {P6X, P6Y, P6Z}
        };
        const int zero = 0;

        const MovementTable standay_table {&kStandby, 1, 20, &zero, 1};
    }

    const MovementTable& standbyTable() {
        return standay_table;
    }

    #include "movement_table.h"
}
